/*
Panoformer Tool 1.0
Library to convert between different panorama formats


Copyright (c) 2013, Ulf Biallas
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/



#include "Panoformer.hh"

#define PI 3.141592654

using namespace std;
using namespace cv;



// Method to convert an equirectangular panorama into a cubic panorama.
// Writes the 6 images of the cubic panorama in the vector imagesPanoCubic (Order: px, nx, py, ny, pz, nz).
void Panoformer::equirectangularToCubic(const Mat& imgPanoEquirectangular, vector<Mat>& imagesPanoCubic) {
		int k, ix, iy, sz;
		float x, y, r, phi, theta;
		float p[3];
		sz = imagesPanoCubic[0].rows;

		int mapping_x[] = { 2,  2,  0,  0,  0,  0};
		int mapping_y[] = { 1,  1,  2,  2,  1,  1};
		int mapping_z[] = { 0,  0,  1,  1,  2,  2};
		int value_z[]   = { 1, -1, -1,  1, -1,  1};
		int flipX[]     = { 1, -1,  1,  1,  1, -1};
		int flipY[]     = { 1,  1, -1,  1,  1,  1};

		for(k=0; k<imagesPanoCubic.size() && k<6; ++k) {
			for(iy = 0; iy<sz; ++iy) {
				for(ix = 0; ix<sz; ++ix) {

					x = 2 * ix / ((float) (sz-1)) - 1;
					y = 2 * iy / ((float) (sz-1)) - 1;

					p[mapping_x[k]] = flipX[k]*x;
					p[mapping_y[k]] = flipY[k]*y;
					p[mapping_z[k]] = value_z[k];

					cartesianToEquirectangularalCoords(p[0], p[1], p[2], phi, theta);

					imagesPanoCubic[k].at<Vec3b>(iy, ix) = getPointOfEquirectangularPanorama(imgPanoEquirectangular, phi, theta);
				}
			}
		}
}


// Method to convert an equirectangular panorama into a little-planet panorama.
void Panoformer::equirectangularToLittleplanet(const Mat& imgPanoEquirectangular, Mat& imgPanoLittleplanet) {
	int ix, iy, x, y;
	float r, phi, sx, sy, maxdist;
	maxdist = 0.5 * min(imgPanoLittleplanet.rows, imgPanoLittleplanet.cols);
	for(iy = 0; iy<imgPanoLittleplanet.rows; ++iy) {
		for(ix = 0; ix<imgPanoLittleplanet.cols; ++ix) {

			x = ix - 0.5 * imgPanoLittleplanet.cols;
			y = iy - 0.5 * imgPanoLittleplanet.rows;

			phi = atan2((float) y, (float) x) * 180 / PI + 180;
			r = sqrt((float) (x*x + y*y));
			if(r>maxdist) r = maxdist;

			sx = phi / 360.0f * (imgPanoEquirectangular.cols-1);
			sy = (imgPanoEquirectangular.rows-1) - r / maxdist * (imgPanoEquirectangular.rows-1);

			imgPanoLittleplanet.at<Vec3b>(iy, ix) = getColorSubpix(imgPanoEquirectangular, Point(sx, sy));
		}
	}
}


// Method to convert a cubic panorama into an equirectangular panorama.
// The vector imagesPanoCubic has to contain the 6 images of a cubic panorama (Order: px, nx, py, ny, pz, nz).
void Panoformer::cubicToEquirectangular(const vector<Mat>& imagesPanoCubic, Mat& imgPanoEquirectangular) {
	int ix, iy;
	float x, y, z;
	float phi, theta;
	for(iy = 0; iy<imgPanoEquirectangular.rows; ++iy) {
		for(ix = 0; ix<imgPanoEquirectangular.cols; ++ix) {

			phi = 360.0f - ix / ((float) (imgPanoEquirectangular.cols-1)) * 360.0f;
			theta = 90.0f - iy / ((float) (imgPanoEquirectangular.rows-1)) * 180;

			equirectangularalToCartesianCoords(1, phi, theta, x, y, z);

			imgPanoEquirectangular.at<Vec3b>(iy, ix) = getPointOfCubicPanorama(imagesPanoCubic, x, y, z);
		}
	}
}


// Method to convert a cubic panorama into a little-planet panorama.
// The vector imagesPanoCubic has to contain the 6 images of a cubic panorama (Order: px, nx, py, ny, pz, nz).
void Panoformer::cubicToLittleplanet(const vector<Mat>& imagesPanoCubic, Mat& imgPanoLittleplanet) {
	int ix, iy, x, y;
	float r, phi, theta, sx, sy, sz, maxdist;
	maxdist = 0.5 * min(imgPanoLittleplanet.rows, imgPanoLittleplanet.cols);
	for(iy = 0; iy<imgPanoLittleplanet.rows; ++iy) {
		for(ix = 0; ix<imgPanoLittleplanet.cols; ++ix) {

			x = ix - 0.5 * imgPanoLittleplanet.cols;
			y = iy - 0.5 * imgPanoLittleplanet.rows;

			phi = atan2((float) y, (float) x) * 180 / PI + 180;
			r = sqrt((float) (x*x + y*y));
			if(r>maxdist) r = maxdist;
			theta = 180 * r / maxdist - 90;

			equirectangularalToCartesianCoords(1, phi, theta, sx, sy, sz);

			imgPanoLittleplanet.at<Vec3b>(iy, ix) = getPointOfCubicPanorama(imagesPanoCubic, sx, sy, sz);
		}
	}
}


// Method to convert a little-planet panorama into an equirectangular panorama.
void Panoformer::littleplanetToEquirectangular(const Mat& imgPanoLittleplanet, Mat& imgPanoEquirectangular) {
	float maxdist = 0.5 * min(imgPanoLittleplanet.rows, imgPanoLittleplanet.cols);
	int ix, iy, x, y;
	float phi, r;
	for(iy = 0; iy<imgPanoEquirectangular.rows; ++iy) {
		for(ix = 0; ix<imgPanoEquirectangular.cols; ++ix) {

			phi = 360.0f - ix / ((float) (imgPanoEquirectangular.cols-1)) * 360.0f;
			r = maxdist - iy / ((float) (imgPanoEquirectangular.rows-1)) * maxdist;

			x = r * cos((phi+180) / 180 * PI) + 0.5 * imgPanoLittleplanet.cols;
			y = r * sin(phi / 180 * PI) + 0.5 * imgPanoLittleplanet.rows;

			imgPanoEquirectangular.at<Vec3b>(iy, ix) = getColorSubpix(imgPanoLittleplanet, Point(x, y));
		}
	}
}


// Method to convert a little-planet panorama into a cubic panorama.
// Writes the 6 images of the cubic panorama in the vector imagesPanoCubic (Order: px, nx, py, ny, pz, nz).
void Panoformer::littleplanetToCubic(const Mat& imgPanoLittleplanet, vector<Mat>& imagesPanoCubic) {

		int k, ix, iy, sz;
		float x, y, r, phi, theta;
		float p[3];
		sz = imagesPanoCubic[0].rows;

		int mapping_x[] = {  2,  2,  0,  0,  0,  0};
		int mapping_y[] = {  1,  1,  2,  2,  1,  1};
		int mapping_z[] = {  0,  0,  1,  1,  2,  2};
		int value_z[]   = {  1, -1,  1, -1,  1, -1};
		int flipX[]     = { -1,  1,  1,  1,  1, -1};
		int flipY[]     = { -1, -1,  1, -1, -1, -1};

		for(k=0; k<imagesPanoCubic.size() && k<6; ++k) {
			for(iy = 0; iy<sz; ++iy) {
				for(ix = 0; ix<sz; ++ix) {

					x = 2 * ix / ((float) (sz-1)) - 1;
					y = 2 * iy / ((float) (sz-1)) - 1;

					p[mapping_x[k]] = flipX[k]*x;
					p[mapping_y[k]] = flipY[k]*y;
					p[mapping_z[k]] = value_z[k];

					cartesianToEquirectangularalCoords(p[0], p[1], p[2], phi, theta);

					imagesPanoCubic[k].at<Vec3b>(iy, ix) = getPointOfLittleplanetPanorama(imgPanoLittleplanet, phi, theta);
				}
			}
		}
}


// read a pixel with bilinear interpolation
Vec3b inline Panoformer::getColorSubpix(const Mat& img, Point2f pt) {
    Mat patch;
    getRectSubPix(img, Size(1,1), pt, patch);
    return patch.at<Vec3b>(0,0);
}



Vec3b inline Panoformer::getPointOfLittleplanetPanorama(const Mat& imgPanoLittleplanet, float& phi, float& theta) {
	float maxdist = 0.5 * min(imgPanoLittleplanet.rows, imgPanoLittleplanet.cols);
	float r = (90 + theta) / 180.0f * maxdist;
	float x = r * cos((phi+180) / 180 * PI) + 0.5 * imgPanoLittleplanet.cols;
	float y = r * sin(phi / 180 * PI) + 0.5 * imgPanoLittleplanet.rows;
	return getColorSubpix(imgPanoLittleplanet, Point(x, y));
}



Vec3b inline Panoformer::getPointOfEquirectangularPanorama(const Mat& imgPanoEquirectangular, float& phi, float& theta) {
	float x = phi / 360.0f * (imgPanoEquirectangular.cols-1);
	float y = (theta+90.0f) / 180.0f * (imgPanoEquirectangular.rows-1);
	return getColorSubpix(imgPanoEquirectangular, Point(x, y));
}



Vec3b inline Panoformer::getPointOfCubicPanorama(const vector<Mat>& imagesPanoCubic, float& x, float& y, float& z) {

	int img, sz, idx_x, idx_y, idx_z = getIdxOfLargestComponent(x, y, z);
	sz = imagesPanoCubic[0].rows-1;
	float p[] = {x, y, z};
	float idx_z_abs = abs(p[idx_z]);
	for(int k=0; k<3; ++k) p[k] /= idx_z_abs;

	float xx, yy;

	switch(idx_z) {
		case 0:
			img = p[idx_z] > 0 ? 5 : 4;
			idx_x = 2;
			idx_y = 1;
			xx = p[idx_z] > 0 ? -p[idx_x] : p[idx_x];
			yy = -p[idx_y];
			break;
		case 1:
			img = p[idx_z] > 0 ? 2 : 3;
			idx_x = 2;
			idx_y = 0;
			xx = p[idx_x];
			yy = p[idx_z] > 0 ? -p[idx_y] : p[idx_y];
			break;
		case 2:
			img = p[idx_z] > 0 ? 0 : 1;
			idx_x = 0;
			idx_y = 1;
			xx = p[idx_z] > 0 ? p[idx_x] : -p[idx_x];
			yy = -p[idx_y];
			break;
	}

	xx = (xx+1) / 2.0f * sz;
	yy = (yy+1) / 2.0f * sz;

	if(img < imagesPanoCubic.size()) {
		return getColorSubpix(imagesPanoCubic[img], Point(xx, yy));
	} else {
		return Vec3b(0.0f, 0.0f, 0.0f);
	}
}



void inline Panoformer::cartesianToEquirectangularalCoords(const float x, const float y, const float z, float& phi, float& theta) {
	float r = sqrt(x*x + y*y + z*z);
	phi =  180 + 180*atan2(z, x) / PI;
	theta = 180.0f * asin(y/r) / PI;
}



void inline Panoformer::equirectangularalToCartesianCoords(const float r, const float phi, const float theta, float& x, float& y, float& z ) {
	float phi2 = phi-90;
	y = r * sin(theta / 180.0f * PI);
	x = r * cos(theta / 180.0f * PI) * cos(phi2 / 180.0f * PI);
	z = r * cos(theta / 180.0f * PI) * sin(phi2 / 180.0f * PI);
}


// return the component with the largest absolute value
int inline Panoformer::getIdxOfLargestComponent(float& x, float& y, float& z) {
	float ax = abs(x);
	float ay = abs(y);
	float az = abs(z);

	if(ax >= ay && ax >= az) return 0;
	if(ay >= ax && ay >= az) return 1;
	if(az >= ax && az >= ay) return 2;
}